/*
 * Queue.c: Implementation of a wait free queue
 */
#include "HPQueue.h"
#include "CAS.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

inline HPQueue* getQueue(unsigned int numOfThreads){
	HPQueue* queue = NULL;
	queue = (HPQueue*)malloc(sizeof(HPQueue));
	memset(queue, 0, sizeof(HPQueue));
	Node* dummyNode = (Node*)malloc(sizeof(Node));
	memset(dummyNode, 0, sizeof(Node));
	dummyNode->value = -1;
	queue->head = queue->tail = dummyNode;
	queue->numOfThreads = numOfThreads;
	queue->memoryManager = getMemoryManager(numOfThreads);
	return queue;
}

inline void deleteQueue(HPQueue* queue){
	if (NULL == queue) return;

	Node* currNode;
	while(NULL != queue->head){
		currNode = queue->head;
		queue->head = queue->head->next;
		free(currNode);
	}
	deleteMemoryManager(queue->memoryManager);
	free(queue);
}

/**
 * Enqueue a new value to the queue
 */
inline void enq(HPQueue* queue, long long myTid, int value) {
	if (NULL == queue) return;
	if (queue->numOfThreads <= myTid) return;

	//Allocate new node
	Node* node = (Node*)malloc(sizeof(Node));
	node->value = value;
	node->next = NULL;

	MemoryManager* mm = queue->memoryManager;

	Node *t, *next;
	while (true){
		t = queue->tail;
		GET_THREAD_DATA(myTid).hp0 = t;
		if (t != queue->tail)
			continue;
		next = t->next;
		if (queue->tail != t)
			continue;
		if (next != NULL){
			COUNT_CAS;
			CAS(&queue->tail, t, next);
			continue;
		}
		COUNT_CAS;
		if (CAS(&t->next, NULL, node))
			break;
	}
	COUNT_CAS;
	CAS(&queue->tail, t, node);

	cleanHP(GET_THREAD_DATA(myTid));
}

/**
 * Dequeue a node from the queue
 */
inline int deq(HPQueue* queue, long long myTid){
	int data = -1;
	if (queue->numOfThreads <= myTid) return data;

	MemoryManager* mm = queue->memoryManager;

	Node *h, *t, *next;
	while (true){
		h = queue->head;
		GET_THREAD_DATA(myTid).hp0 = h;
		if (queue->head != h)
			continue;
		t = queue->tail;
		next = h->next;
		GET_THREAD_DATA(myTid).hp1 = next;
		if (queue->head != h)
			continue;
		if (next==NULL)
			break;
		if (h==t){
			COUNT_CAS;
			CAS(&queue->tail, t, next);
			continue;
		}
		data = next->value;
		COUNT_CAS;
		if (CAS(&queue->head, h, next)){
			retireNode(queue->memoryManager, h, myTid);
			break;
		}
	}

	cleanHP(GET_THREAD_DATA(myTid));
	return data;
}

inline void printQueue(HPQueue* queue){
	Node* node = queue->head->next;
	printf("Queue = ");
	while(node){
		printf("%d, ",node->value);
		node = node->next;
	}
	printf("\n");
}

inline void printQueueSize(HPQueue* queue){
	Node* node = queue->head->next;
	int size = 0;
	while(node){
		size++;
		node = node->next;
	}
	printf("queue size=%d\n",size);
}
